#ifndef REGISTRATIONIMPL_H
#define REGISTRATIONIMPL_H
#include "coarse_registration.h"
#include "dox.h"
#include "icp.h"
#include "point_cloud_normal.h"
#include "tsdf.h"
#include <opencv2/opencv.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

namespace Registration {

class RegistrationImpl {
public:
  RegistrationImpl();
  ~RegistrationImpl();
  void SetCameraParams(Registration::CameraParams &camera_params_);
  void InitRegistration();
  void
  SetInputSourcePointCloud(pcl::PointCloud<pcl::PointXYZ> &input_point_cloud);
  // void SetInputTargetPointCloud();
  void ComputePointCloudNormal();
  void Reset();
  void IcpRegistration();
  void Initialize();
  void Run();

public:
  Dox *dox_;

private:
  void
  GetNormalDirection(pcl::PointCloud<pcl::Normal>::Ptr &point_with_normals);
  void DownsamplePointCloud(); // down_sample point cloud
private:
  CameraParams camera_params;
  CoraseRegistration coarse_registration;
  int point_cloud_count;

  // pcl::PointCloud<pcl::PointXYZINormal>::Ptr
  // std::vector<cv::Mat> total_depth_map;
};

} // namespace Registration

#endif